{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from rei_roslaunch import robot_launch,ReiRoslaunch\n",
    "import os\n",
    "\n",
    "class robot_startup():\n",
    "    def __init__(self,rei_robot=None):\n",
    "        if rei_robot == None:\n",
    "            try:\n",
    "                self.rei_robot = os.environ['REI_ROBOT']\n",
    "            except:\n",
    "                print('未找到设备信息，请自定义')\n",
    "        else:\n",
    "            self.rei_robot = rei_robot\n",
    "        self.log_level = ['INFO','WARN','ERROR']\n",
    "        self.cfg = self._read_json_file()\n",
    "        self.devList = os.popen('ls /dev').read().split('\\n')\n",
    "        self.runList = []\n",
    "\n",
    "    def _read_json_file(self):\n",
    "        import json\n",
    "        try:\n",
    "            with open('../src/config/reirobot.json', 'r') as file:\n",
    "                data = json.load(file)\n",
    "                return data\n",
    "        except FileNotFoundError:\n",
    "            print(\"config/reirobot.json:文件未找到\")\n",
    "        except json.JSONDecodeError:\n",
    "            print(\"config/reirobot.json:文件JSON数据解析错误\")\n",
    "        return data\n",
    "\n",
    "    def open_navigation(self,map_name):\n",
    "        self.shutdown()        \n",
    "        launchlist = []\n",
    "        nav = self.cfg['robotConfig'][self.rei_robot]['nav']\n",
    "        nav['args'].append('map_file_name:='+map_name)\n",
    "        launchlist.append({'pkg':'rosbridge_server','file':'rosbridge_websocket.launch','args':[]},)\n",
    "        launchlist.append(nav)\n",
    "        launchlist.append({'pkg':'relative_move',\n",
    "                           'file':'relative_move.launch',\n",
    "                           'args':[]})\n",
    "        if 'base_camera' in self.devList:\n",
    "            launchlist.append({'pkg':'ar_pose',\n",
    "                               'file':'ar_base.launch',\n",
    "                               'args':[]})\n",
    "        else:\n",
    "            self.pub_log('未检测到底盘相机',1)\n",
    "        # if self.devList == 'hand_camera':\n",
    "        #     launchlist.append({'pkg':'ar_pose',\n",
    "        #                        'file':'ar_base',\n",
    "        #                        'args':[]})\n",
    "        #     if self.devList == 'ttyACM0':\n",
    "        #         self.pub_log('检测到机械臂')\n",
    "        #         launchlist.append({'pkg':'arm_controller',\n",
    "        #                         'file':'pick_ar.launch',\n",
    "        #                         'args':[]})\n",
    "        # else:\n",
    "        #     if self.devList == 'ttyACM0':\n",
    "        #         self.pub_log('检测到机械臂')\n",
    "        #         launchlist.append({'pkg':'arm_controller',\n",
    "        #                         'file':'arm_controller.launch',\n",
    "        #                         'args':[]})\n",
    "\n",
    "        ros_nav = ReiRoslaunch(launchlist,is_core=True)\n",
    "        ros_nav.start()\n",
    "        self.pub_log('rosbridge_server启动成功')\n",
    "        ros_nav.start_node('tf2_web_republisher','tf2_web_republisher','tf2_web_republisher')\n",
    "        self.runList.append(ros_nav)\n",
    "        self.pub_log('tf2_web启动成功')\n",
    "        return True\n",
    "    \n",
    "    def open_slam(self):\n",
    "        self.shutdown()\n",
    "        launchlist = []\n",
    "        slam = self.cfg['robotConfig'][self.rei_robot]['slam']\n",
    "        launchlist.append({'pkg':'rosbridge_server','file':'rosbridge_websocket.launch','args':[]},)\n",
    "        launchlist.append(slam)\n",
    "        ros_slam = ReiRoslaunch(launchlist,is_core=True)\n",
    "        ros_slam.start()\n",
    "        ros_slam.start_node('tf2_web_republisher','tf2_web_republisher','tf2_web_republisher')\n",
    "        self.runList.append(ros_slam)\n",
    "\n",
    "    def shutdown(self):\n",
    "        for ros in self.runList:\n",
    "            if ros.getState() == \"ACTIVE\":\n",
    "                ros.shutdown()\n",
    "        self.runList = []\n",
    "        # self.pub_log('roscore已经关闭')\n",
    "        return False\n",
    "        \n",
    "    def save_map(self,map_name):\n",
    "        if self.getState() == \"ACTIVE\":\n",
    "            launch = self.cfg['robotConfig'][self.rei_robot]['map_save']\n",
    "            launch['args'].append('map_file:='+map_name)\n",
    "            ros = ReiRoslaunch([launch])\n",
    "            ros.start()\n",
    "            self.runList.append(ros)\n",
    "    \n",
    "    def getState(self):\n",
    "        for ros in self.runList:\n",
    "            return ros.getState()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = robot_startup()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.open_slam()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.save_map('test5556666')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.shutdown()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.getState()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
